#include "bsp_servo.h"
#include "bsp_SysTick.h"
/**==================================================================================================================
 *              舵机PWM信号要求：周期20ms; 0.5ms脉宽时舵臂角度为0度; 2.5ms脉宽时舵臂角度为180度。
 *
                ！！！注意舵臂转动是需要时间的，重设脉宽后，舵臂需要一定的时间才能转动到目标角度。！！！

 *【 注  意 】  1_STM32F407, 在默认的系统频率168MHz下，TIM1、8、9、10、11的时钟频率是APB2*2=84MHz*2=168MHz,
                而TIM2、3、4、5、6、7、12、13、14的时钟频率是APB1*2=42MHz*2=84MHz;
 *              2_PSC控制1脉冲时长; ARR用脉冲个数控制周期时长; CCR决定占空比; CNT值每1个脉冲自动递增或递减，硬件会把它与CCR比大小以产生高低电平;
 *              3_PSC: 预分频; 作用：控制计数器每一脉冲的时长; 解释：把接口时钟，分频后提供给计数器使用，即多少个接口时钟脉冲，才产生一次计数器脉冲; 所有TIM的PSC都是16位, 取值范围:1~65535;
 *              4_ARR: 自动重载值; 作用：控制周期; 解释：多少个计数器脉冲，组成一波形周期;
 *              5_CCR: 比较寄存器; 作用：控制脉宽; 解释：很重要的概念，比较，硬件会把CNT值与CCR比较大小，以产生有效电平;
 *              6_输出极性，可理解为有效电平，很多人不理解. 在TIM_OCPolarity里设置，可以设置为高、低电平; 如，当PWM1模式下，当CNT<CCR时输出有效电平，这个有效电平，就是你设置的“输出极性”; 这配置，没特别的技术含意，只是为了更灵活控制;
 *              7_TIM2和TIM5比较特殊，PSC是16位的，ARR、CNT、CCR这三个寄存器，是32位的，可以实现更方便的控制。
                但是，没特殊需求时，还是把它们当16位一般使用吧，以使程序更方便移植;

 *              8_关于如何计算分频值PSC和自动重载值ARR，没技巧，多尝试计算几次才能理解。
                通过函数 TIM_SetCompare1(TIMx, 脉冲数), 可修改脉宽;
                可以尝试使用上述中的脉冲数来理解，主要是三个值：分频后的计数器1脉冲时长，多少个脉冲时长组成1周期，CCR比较值是多少个脉冲时长;
====================================================================================================================*/

void TIM5_PWM_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;           // 定义引脚实始化结构体
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; // 定义时基结构体
    TIM_OCInitTypeDef TIM_OCInitStructure;         // 定义通道模式配置结构体

    /** 使能相关时钟 **/
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOI | RCC_AHB1Periph_GPIOH, ENABLE); // 开启相关的GPIO外设时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);                         // 使能定时器时钟
    TIM_DeInit(TIM5);

    /** 配置所用TIM的时基 **/
    TIM_TimeBaseStructure.TIM_Prescaler     = 84 - 1;             // 分频; 把接口时钟分频后给计数器使用, 即多少个接口脉冲，才产生一次计数器脉冲; 简单理解：计算每一计数器脉冲的时长;
    TIM_TimeBaseStructure.TIM_Period        = 20000 - 1;          // ARR, 自动重载值; 多少个计数器脉冲作为一周期; 注意：TIM2和5是32位的，其它TIM是16位的;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;       // 采样时钟分频
    TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up; // 计数方式
    TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);               // 初始化定时器，把配置写入寄存器

    /** 配置TIM5通道1,2,3所用引脚 **/
    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12; // 引脚编号
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;                            // 工作模式：复用模式
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;                           // 输出类型：推挽模式
    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;                            // 上下拉选择：浮空
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;                       // 端口速率：100MHz
    GPIO_Init(GPIOH, &GPIO_InitStructure);                                   // 初始化，把结构体数据写入到芯片寄存器
                                                                             /** 配置通道1,2,3所用引脚的复用功能 **/
    GPIO_PinAFConfig(GPIOH, GPIO_PinSource10, GPIO_AF_TIM5);                 // 定时器通道引脚复用
    GPIO_PinAFConfig(GPIOH, GPIO_PinSource11, GPIO_AF_TIM5);                 // 定时器通道引脚复用
    GPIO_PinAFConfig(GPIOH, GPIO_PinSource12, GPIO_AF_TIM5);                 // 定时器通道引脚复用

    /** 配置TIM5通道4所用引脚 **/
    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_0;        // 引脚编号
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;      // 工作模式：复用模式
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;     // 输出类型：推挽模式
    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;  // 上下拉选择：浮空
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; // 端口速率：100MHz
    GPIO_Init(GPIOI, &GPIO_InitStructure);             // 初始化，把结构体数据写入到芯片寄存器
    /** 配置通道4所用引脚的复用功能 **/
    GPIO_PinAFConfig(GPIOI, GPIO_PinSource0, GPIO_AF_TIM5); // 定时器通道引脚复用

    /** 配置通道1,2,3,4的工作模式 **/
    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM2;        // 比较输出方式; 简单理解: PWM1_CNT<CCR时产生有效电平、PWM2_CNT>CCR时产生有效电平;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 通道输出开关;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    // TIM_OCInitStructure.TIM_Pulse = 0;                           // 比较值; 简单理解：计数器每次脉冲均把CNT值与此CCR值比较大小，根据设置的工作模式TIM_OCMode值，用于产生有效电平(脉宽);
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; // 输出极性; 可配置为：高、低电平; 简单理解:有效电平，需配合上面的TIM_OCMode选项来理解;
                                                             /* 初始化，把上面参数写到寄存器 */
    TIM_OC1Init(TIM5, &TIM_OCInitStructure);
    TIM_OC2Init(TIM5, &TIM_OCInitStructure); // 初始化，把上面参数写到寄存器
    TIM_OC3Init(TIM5, &TIM_OCInitStructure); // 初始化，把上面参数写到寄存器
    TIM_OC4Init(TIM5, &TIM_OCInitStructure); // 初始化，把上面参数写到寄存器
    /* 配置通道预装载; 理解：Enable_修改CCR值(占空比)后等下个周期再生效、Disable_修改CCR值(占空比)后立即生效 */
    TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);
    TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable); // 配置通道预装载; 理解：Enable_修改CCR值(占空比)后等下个周期再生效、Disable_修改CCR值(占空比)后立即生效;
    TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable); // 配置通道预装载; 理解：Enable_修改CCR值(占空比)后等下个周期再生效、Disable_修改CCR值(占空比)后立即生效;
    TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable); // 配置通道预装载; 理解：Enable_修改CCR值(占空比)后等下个周期再生效、Disable_修改CCR值(占空比)后立即生效;
    TIM_ARRPreloadConfig(TIM5, ENABLE);               // ARPE使能
    TIM_SetCompare1(TIM5, 0);
    TIM_SetCompare2(TIM5, 0);
    TIM_SetCompare3(TIM5, 0);
    TIM_SetCompare4(TIM5, 0);
    /** 使能定时器 **/
    TIM_Cmd(TIM5, ENABLE); // 使能定时器
}
void TIM2_PWM_Init(void)
{
    /** 使能相关时钟 **/
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); // 开启相关的GPIO外设时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);  // 使能定时器时钟
    TIM_DeInit(TIM2);
    /** 配置所用TIM的时基 **/
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;                // 定义时基结构体
    TIM_TimeBaseStructure.TIM_Prescaler     = 84 - 1;             // 分频; 把接口时钟分频后给计数器使用, 即多少个接口脉冲，才产生一次计数器脉冲; 简单理解：计算每一计数器脉冲的时长;
    TIM_TimeBaseStructure.TIM_Period        = 20000 - 1;          // ARR, 自动重载值; 多少个计数器脉冲作为一周期; 注意：TIM2和5是32位的，其它TIM是16位的;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;       // 采样时钟分频
    TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up; // 计数方式
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);               // 初始化定时器，把配置写入寄存器
    /** 配置通道3所用引脚 **/
    GPIO_InitTypeDef GPIO_InitStructure;                     // 定义引脚实始化结构体
    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10;             // 引脚编号
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;            // 工作模式：复用模式
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;           // 输出类型：推挽模式
    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_DOWN;          // 上下拉选择：浮空
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;       // 端口速率：100MHz
    GPIO_Init(GPIOB, &GPIO_InitStructure);                   // 初始化，把结构体数据写入到芯片寄存器
                                                             /** 配置通道3所用引脚的复用功能 **/
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_TIM2); // 定时器通道引脚复用
    /** 配置通道3的工作模式 **/
    TIM_OCInitTypeDef TIM_OCInitStructure;                        // 定义通道模式配置结构体
    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM2;        // 比较输出方式; 简单理解: PWM1_CNT<CCR时产生有效电平、PWM2_CNT>CCR时产生有效电平;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 通道输出开关;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    // TIM_OCInitStructure.TIM_Pulse = 0;                             // 比较值; 简单理解：计数器每次脉冲均把CNT值与此CCR值比较大小，根据设置的工作模式TIM_OCMode值，用于产生有效电平(脉宽);
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; // 输出极性; 可配置为：高、低电平; 简单理解:有效电平，需配合上面的TIM_OCMode选项来理解;
    TIM_OC3Init(TIM2, &TIM_OCInitStructure);                 // 初始化，把上面参数写到寄存器
                                                             /** 配置通道3预装载 **/
    TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);        // 配置通道预装载; 理解：Enable_修改CCR值(占空比)后等下个周期再生效、Disable_修改CCR值(占空比)后立即生效;

    /** 配置通道4所用引脚 **/
    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_11;             // 引脚编号
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;            // 工作模式：复用模式
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;           // 输出类型：推挽模式
    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;        // 上下拉选择：浮空
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;       // 端口速率：100MHz
    GPIO_Init(GPIOB, &GPIO_InitStructure);                   // 初始化，把结构体数据写入到芯片寄存器
                                                             /** 配置通道4所用引脚的复用功能 **/
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_TIM2); // 定时器通道引脚复用
    /** 配置通道4的工作模式 **/
    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;        // 比较输出方式; 简单理解: PWM1_CNT<CCR时产生有效电平、PWM2_CNT>CCR时产生有效电平;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 通道输出开关;
    TIM_OCInitStructure.TIM_Pulse       = 0;                      // 比较值; 简单理解：计数器每次脉冲均把CNT值与此CCR值比较大小，根据设置的工作模式TIM_OCMode值，用于产生有效电平(脉宽);
    TIM_OCInitStructure.TIM_OCPolarity  = TIM_OCPolarity_High;    // 输出极性; 可配置为：高、低电平; 简单理解:有效电平，需配合上面的TIM_OCMode选项来理解;
    TIM_OC4Init(TIM2, &TIM_OCInitStructure);                      // 初始化，把上面参数写到寄存器
                                                                  /** 配置通道4预装载 **/
    TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);

    TIM_ARRPreloadConfig(TIM2, ENABLE); // ARPE使能
                                        /** 使能定时器 **/
    TIM_Cmd(TIM2, ENABLE);              // 使能定时器
}
/**
*  大小舵机角度控制 不带延时
*  @name Servo_Set
*  @para number 舵机编号1-6
                1-> PB10 右臂 600  朝下 1500水平 调节范围600->1500
                2-> PB11 左臂 2300 朝下 1400水平 调节范围2300->1400
                6-> PI0  前铲180度舵机 950 水平 1900竖直 调节范围950->1900
*  @para angle 角度
*               1-> PB10 右臂 600  朝下 1500水平 调节范围600->1500
                2-> PB11 左臂 2300 朝下 1400水平 调节范围2300->1400
                6-> PI0  前铲180度舵机 950 水平 1900竖直 调节范围950->1900
**/
void servo(uint8_t num, uint32_t angle)
{
    switch (num) {
        case 1:
            TIM_SetCompare3(TIM2, angle);
            break;
        case 2:
            TIM_SetCompare4(TIM2, angle);
            break;
        case 3:
            TIM_SetCompare1(TIM5, angle);
            break;
        case 4:
            TIM_SetCompare2(TIM5, angle);
            break;
        case 5:
            TIM_SetCompare3(TIM5, angle);
            break;
        case 6:
            TIM_SetCompare4(TIM5, angle);
            break;
        default:
            TIM_SetCompare3(TIM2, 0);
            TIM_SetCompare4(TIM2, 0);
            break;
    }
}

/**
*  大小舵机角度控制 带延时
*  @name Servo_Set
*  @param number 舵机编号1-6
                1-> PB10 右臂 600  朝下 1500水平 调节范围600->1500
                2-> PB11 左臂 2300 朝下 1400水平 调节范围2300->1400
                6-> PI0  前铲180度舵机 950 水平 1900竖直 调节范围950->1900
*  @param angle 角度
*               1-> PB10 右臂 600  朝下 1500水平 调节范围600->1500
                2-> PB11 左臂 2300 朝下 1400水平 调节范围2300->1400
                6-> PI0  前铲180度舵机 950 水平 1900竖直 调节范围950->1900
**/
void Servo_Set(uint8_t number, uint32_t angle)
{
    switch (number) {
        case 1:
            TIM_SetCompare3(TIM2, angle);
            delay_ms(80);
            break;
        case 2:
            TIM_SetCompare4(TIM2, angle);
            delay_ms(80);
            break;
        case 3:
            TIM_SetCompare1(TIM5, angle);
            delay_ms(100);
            break;
        case 4:
            TIM_SetCompare2(TIM5, angle);
            delay_ms(100);
            break;
        case 5:
            TIM_SetCompare3(TIM5, angle);
            delay_ms(100);
            break;
        case 6:
            TIM_SetCompare4(TIM5, angle);
            delay_ms(80);
            break;
        default:
            TIM_SetCompare3(TIM2, 0);
            TIM_SetCompare4(TIM2, 0);
            break;
    }
}
/**
 * 初始化所有舵机--手臂放下
 */
void Setup_AllServo(void)
{
    Servo_Set(1, 2300);
    Servo_Set(2, 600);
    Servo_Set(6, 850);
    Servo_Set(5, 1500);
    delay_ms(100);

    Servo_Set(1, 0);
    Servo_Set(2, 0);
}
/**
 * 先后挥动双手
 */
void Wave_Hands(void)
{
    Servo_Set(1, 1300);
    delay_ms(100);
    Servo_Set(1, 600);
    Servo_Set(2, 1600);
    delay_ms(100);
    Servo_Set(2, 2300);
}
/**
 * 同时举起双臂
 */
void LiftUp_Hands(void)
{
    // Servo_Set(1,1300);
    TIM_SetCompare3(TIM2, 1300);
    // Servo_Set(2,1600);
    TIM_SetCompare4(TIM2, 1600);
    delay_ms(200);
    // Servo_Set(1,600);
    TIM_SetCompare3(TIM2, 600);
    // Servo_Set(2,2300);
    TIM_SetCompare4(TIM2, 2300);
    delay_ms(200);
    Servo_Set(1, 0);
    Servo_Set(2, 0);
}
void Front_down(void)
{
    servo(6, 850);
}
void Front_mid(void)
{
    servo(6, 2100);
}
void Front_up(void)
{
    servo(6, 800);
}
void Front_up_High(void)
{
    servo(6, 400);
}